/*
 * My_Usart.c
 *
 *  Created on: Mar 25, 2024
 *      Author: Yiwen Zhu
 */

#include "My_Usart.h"
#include "Motor_Control_Init.h"
#include "arm_math.h"


uint8_t USART_SEND_BUF[10] = {0};
uint8_t USART_RECV_BUF[15] = {0};

uint16_t Motor_Usart_Data_buf[4]={0};




void init_usart_send_dma(void){
	USART_SEND_BUF[0] = 0x55;
	USART_SEND_BUF[9] = 0xff;
	HAL_UART_Transmit_DMA(&huart1, (uint8_t *)USART_SEND_BUF, sizeof(USART_SEND_BUF));
}

void update_usart_dma(uint16_t* send_buf){

	USART_SEND_BUF[1] = (send_buf[0]>>8);
	USART_SEND_BUF[2] = (send_buf[0])&0xff;
	USART_SEND_BUF[3] = (send_buf[1]>>8);
	USART_SEND_BUF[4] = (send_buf[1])&0xff;
	USART_SEND_BUF[5] = (send_buf[2]>>8);
	USART_SEND_BUF[6] = (send_buf[2])&0xff;
	USART_SEND_BUF[7] = (send_buf[3]>>8);
	USART_SEND_BUF[8] = (send_buf[3])&0xff;

}

void usart_dma_start_once(void){
	HAL_UART_Transmit_DMA(&huart1, (uint8_t *)USART_SEND_BUF, sizeof(USART_SEND_BUF));
}

void motor_usart_data_update(void){
	// ###############  Test  #############


	// ####################################
	Motor_Usart_Data_buf[0] = SEND_DATA_1;
	Motor_Usart_Data_buf[1] = SEND_DATA_2;
	Motor_Usart_Data_buf[2] = SEND_DATA_3;
	Motor_Usart_Data_buf[3] = SEND_DATA_4;

	USART_SEND_BUF[1] = (Motor_Usart_Data_buf[0]>>8);
	USART_SEND_BUF[2] = (Motor_Usart_Data_buf[0])&0xff;
	USART_SEND_BUF[3] = (Motor_Usart_Data_buf[1]>>8);
	USART_SEND_BUF[4] = (Motor_Usart_Data_buf[1])&0xff;
	USART_SEND_BUF[5] = (Motor_Usart_Data_buf[2]>>8);
	USART_SEND_BUF[6] = (Motor_Usart_Data_buf[2])&0xff;
	USART_SEND_BUF[7] = (Motor_Usart_Data_buf[3]>>8);
	USART_SEND_BUF[8] = (Motor_Usart_Data_buf[3])&0xff;

	HAL_UART_Transmit_DMA(&huart1, (uint8_t *)USART_SEND_BUF, sizeof(USART_SEND_BUF));
}

int i_test = 0;
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)   {
	u16 temp = 0;
	u16 start = 0;
	LED1 = ~LED1;
	i_test++;

	for(int j=0;j<6;j++){
		if(USART_RECV_BUF[j]==0x55&&USART_RECV_BUF[j]==0xAA){
			start = j;
			break;
		}
		if(j==5){
			HAL_UART_DMAStop(&huart1);
			HAL_UART_Receive_DMA(&huart1, USART_RECV_BUF,10); // Enable DMA-UART RX
		}
	}


	switch(USART_RECV_BUF[start+2]){
//				case 0x01:
//					temp = ((u16)USART_RECV_BUF[3]<<8)+USART_RECV_BUF[4];
//					pi_pll1.Kp = _IQ(temp/1000.0);
//					break;
//				case 0x02:
//					temp = ((u16)USART_RECV_BUF[3]<<8)+USART_RECV_BUF[4];
//					pi_pll1.Ki = _IQ(temp/1000.0);
//					break;
//				case 0x03:
//					temp = ((u16)USART_RECV_BUF[3]<<8)+USART_RECV_BUF[4];
//					ANG_SET_VRB = _IQsat(_IQ((temp-1000)/1000.0),_IQ(1),_IQ(0.0));
//					break;
				case 0x06:
					temp = ((u16)USART_RECV_BUF[start+3]<<8)+USART_RECV_BUF[start+4];
					pi_id1.Ref = _IQsat(_IQ((temp-1000)/1000.0),_IQ(1),_IQ(-1.0));
					break;
				case 0x07:
					temp = ((u16)USART_RECV_BUF[start+3]<<8)+USART_RECV_BUF[start+4];
					pi_iq1.Ref = _IQsat(_IQ((temp-1000)/1000.0),_IQ(1),_IQ(-1.0));
					break;
				case 0x08:
					temp = ((u16)USART_RECV_BUF[start+3]<<8)+USART_RECV_BUF[start+4];
					pi_spd1.Ref = _IQsat(_IQ((temp-1000)/1000.0),_IQ(1),_IQ(-1));
					break;
				case 0x09:
					temp = ((u16)USART_RECV_BUF[start+3]<<8)+USART_RECV_BUF[start+4];
					pi_id1.Kp = _IQsat(_IQ((temp)/1000.0),_IQ(10),_IQ(0.0));
					break;
				case 0x0a:
					temp = ((u16)USART_RECV_BUF[start+3]<<8)+USART_RECV_BUF[start+4];
					pi_id1.Ki = _IQsat(_IQ((temp)/1000.0),_IQ(1),_IQ(0.0));
					break;
				case 0x0b:
					temp = ((u16)USART_RECV_BUF[start+3]<<8)+USART_RECV_BUF[start+4];
					pi_iq1.Kp = _IQsat(_IQ((temp)/1000.0),_IQ(10),_IQ(0.0));
					break;
				case 0x0c:
					temp = ((u16)USART_RECV_BUF[start+3]<<8)+USART_RECV_BUF[start+4];
					pi_iq1.Ki = _IQsat(_IQ((temp)/1000.0),_IQ(1),_IQ(0.0));
					break;
				case 0x0d:
					temp = ((u16)USART_RECV_BUF[start+3]<<8)+USART_RECV_BUF[start+4];
					pi_spd1.Kp = _IQ(temp/1000.0);
					break;
				case 0x0e:
					temp = ((u16)USART_RECV_BUF[start+3]<<8)+USART_RECV_BUF[start+4];
					pi_spd1.Ki = _IQ(temp/1000.0);
					break;
//				case 0xfd:
//					temp = ((u16)USART_RECV_BUF[3]<<8)+USART_RECV_BUF[4];
//					smo1.h = _IQ(temp/1000.0);
//					break;
				case 0x05:  STATE_ctl_commond(STOP); LED0 = 1;
					break;
				case 0x04:	STATE_ctl_commond(START); LED0 =0;
					break;
	}

}
